#ifndef VT_EXECUTOR_H
#define VT_EXECUTOR_H

#include <cmath>
#include <iostream>
#include <libplayerc++/playerc++.h>
#include "pause.h"
#include "VelocityTuning.h"
#include "PlayerObjectManager.h"
#include "PeopleCircle.h"
#include "GlobalStructure.h"
#include <algorithm> 

class Executor {
    
    const std::vector<Position>& path;
    PlayerCc::PlayerClient & robot;
    PlayerCc::Position2dProxy & pp;
    PlayerCc::Position2dProxy & gocon;
    PeopleCircle pc;
    
    bool at_position(player_pose2d pose) {
        // TODO: use spaceinformation?
        return (fabs(pose.px - pp.GetXPos()) + fabs(pose.py - pp.GetYPos())) < 0.1;
    }
    
public:
    Executor(const std::vector<Position>& path,
             PlayerCc::PlayerClient & robot,
             PlayerCc::Position2dProxy & pp,
             PlayerCc::Position2dProxy & gocon)
    : path(path), robot(robot), pp(pp), gocon(gocon) {
    }
    
    // execute_plan()
    void execute() {
        std::size_t stateCount = path.size();
        std::vector<Position> npath = path;
        VelocityTuning velTuner(0.5);
        Robot2D current;
        current.x = pp.GetXPos();
        current.y = pp.GetYPos();
        double p = pow(10.0,9.0);
        bool r = true;
        
        std::vector<PlayerObject>& obstacles = PlayerObjectManager::Instance().GetDynamicObjects();

        while (true) {
            pc.RefreshPositions();
            velTuner.updateState(current, npath, obstacles);
            double wait = velTuner.waitTime;
            if (wait > 0) {
                pp.SetSpeed(0,0,0);
                long sec = (long)wait;
                double nv_sec = wait - sec;
                util::pause((long)wait, (long)(nv_sec * p));
            }
            for (int i = 1; i < stateCount; i++) {
                const Position& state = npath[i];
                //            robot.Read();
                player_pose2d waypoint;
                waypoint.px = state.x;
                waypoint.py = state.y;
                waypoint.pa = state.yaw;
                current.x = pp.GetXPos();
                current.y = pp.GetYPos();
                std::cout << "From: " << pp.GetXPos() << "," << pp.GetYPos() << endl;
                std::cout << "Going to (" << waypoint.px << ", " << waypoint.py << ")\n";
                Robot2D nextPos (waypoint.px,waypoint.py);
                Segment2D seg (current, nextPos);
                
//                velTuner.updateState(current, seg, obstacles);
//                std::pair<double, double> sol = velTuner.getSol();
//                if (sol.first > 0) {
//                    pp.SetSpeed(0,0,0);
//                    long sec = (long)sol.first;
//                    double nv_sec = sol.first - sec;
//                    util::pause((long)sol.first, (long)(nv_sec * p));
//                }
//                if (sol.first == 0 && sol.second == 0) {
//                    pp.SetSpeed(0,0,0);
//                    util::pause(5,0);
//                    continue;
//                }
                Robot2D speed = velTuner.getVelocity(seg);
                if (!at_position(waypoint)) {
                    // if not at position, send the goto command
                    //                gocon.GoTo(waypoint);
                    Robot2D pos = current;
                    int i = 0;
                    do {
                        //10 minisecond
                        i++;
                        util::pause(0,100000);
                        robot.Read();
                        pos.x = pp.GetXPos();
                        pos.y = pp.GetYPos();
//                        Robot2D speed = velTuner.getVelocity(i/1000.0);
                        //                    std::cout << "At position: "<<pp.GetXPos() << " , " << pp.GetYPos() << std::endl;
                        //                    std::cout << "At speed: "<< speed.x << " , " << speed.y<<" , "<<speed.yaw << std::endl;
                        
                        pp.SetSpeed(speed.x,speed.y,0.0);
                    } while (!at_position(waypoint));
//                    pp.SetSpeed(0,0,0);
                }
            }
            pp.SetSpeed(0,0,0);
            for (string::size_type t = 0; t < path.size(); ++t) {
                if(r) {
                    npath[t] = path[path.size() - 1 - t];
                } else {
                    npath[t] = path[t];
                }
            }
            r = !r;
        }
    }
};

#endif
